"""
PWM舵机实现
提供PWM舵机的具体控制功能
"""

import threading
import time
from typing import Optional
from .base_servo import ServoInterface
import pigpio


class PWMServo(ServoInterface):
    """PWM舵机类"""

    def __init__(
        self,
        pi: pigpio.pi,
        pin: int,
        freq: int = 50,
        min_width: int = 500,
        max_width: int = 2500,
        deviation: int = 0,
        control_speed: bool = False,
    ):
        """初始化PWM舵机

        Args:
            pi: pigpio实例
            pin: GPIO引脚号
            freq: PWM频率(Hz)
            min_width: 最小脉冲宽度(us)
            max_width: 最大脉冲宽度(us)
            deviation: 角度偏移
            control_speed: 是否启用速度控制
        """
        self.pi = pi
        self.pin = pin
        self.freq = freq
        self.min_width = min_width
        self.max_width = max_width
        self.deviation = deviation
        self.control_speed = control_speed

        # 位置相关
        self.position = 1500  # 当前位置
        self.position_set = self.position  # 目标位置
        self.position_set_t = 0  # 临时目标位置
        self.pos_changed = False  # 位置是否改变

        # 速度控制相关
        self.step_time = 20  # 步长(ms)
        self.position_inc = 0.0  # 位置增量
        self.time = 0  # 当前时间
        self.time_t = 0  # 目标时间
        self.inc_times = 0  # 增量次数
        self.servo_running = False  # 舵机是否运行
        self.speed_limit = 1.0  # 速度限制(0-1)

        # 角度范围
        self.min_angle = 0
        self.max_angle = 180

        # 初始化PWM
        self.pi.set_PWM_range(self.pin, int(1000000 / self.freq))
        self.pi.set_PWM_frequency(self.pin, self.freq)

        # 启动速度控制线程
        if self.control_speed:
            self._start_speed_control_thread()

    def _start_speed_control_thread(self):
        """启动速度控制线程"""
        self.control_thread = threading.Thread(
            target=self._update_position, daemon=True
        )
        self.control_thread.start()

    def set_position(self, position: int, speed: int = 1000) -> None:
        """设置舵机位置

        Args:
            position: 目标位置(500-2500)
            speed: 运动速度(0-1000)
        """
        if position < self.min_width or position > self.max_width:
            print(f"Invalid position: {position}")
            return

        if speed == 0:
            # 直接设置位置
            self.position = position
            self.position_set = position
            self._set_pwm(position)
        else:
            # 使用速度控制
            if speed < 20:
                self.time_t = 20
            elif speed > 30000:
                self.time_t = 30000
            else:
                self.time_t = speed

            self.position_set_t = position
            self.pos_changed = True

    def get_position(self) -> int:
        """获取当前舵机位置"""
        return self.position

    def set_deviation(self, deviation: int) -> None:
        """设置角度偏移

        Args:
            deviation: 偏移量(-300~300)
        """
        if -300 <= deviation <= 300:
            self.deviation = deviation

    def _set_pwm(self, position: int) -> None:
        """设置PWM输出

        Args:
            position: 目标位置
        """
        duty_cycle = int((position + self.deviation) * self.freq / 1000000 * 1000000)
        self.pi.set_PWM_dutycycle(self.pin, duty_cycle)

    def _update_position(self) -> None:
        """更新舵机位置(速度控制)"""
        while True:
            if self.pos_changed:
                self.time = self.time_t
                self.position_set = self.position_set_t
                self.pos_changed = False

                # 计算位置增量
                self.inc_times = self.time / self.step_time
                if self.position_set > self.position:
                    self.position_inc = self.position_set - self.position
                else:
                    self.position_inc = self.position - self.position_set
                self.position_inc = self.position_inc / float(self.inc_times)
                self.servo_running = True

            if self.servo_running:
                self.inc_times -= 1
                if self.inc_times == 0:
                    self.position = self.position_set
                    self.servo_running = False
                else:
                    if self.position_set > self.position:
                        self.position = int(self.position + self.position_inc)
                    else:
                        self.position = int(self.position - self.position_inc)

                # 应用速度限制
                if self.speed_limit < 1.0:
                    self.position = int(self.position * self.speed_limit)

                self._set_pwm(self.position)

            time.sleep(self.step_time / 1000)

    def emergency_stop(self) -> None:
        """紧急停止"""
        self.pi.set_PWM_dutycycle(self.pin, 0)
